#ifndef TRACK_OBJECT_H
#define TRACK_OBJECT_H

#include <deque>
#include <eigen3/Eigen/Core>
#include <queue>
#include <string>
#include <vector>

#include "postprocess/common/common_func.h"
#include "postprocess/common/postprocess_id_generator.h"
#include "postprocess/kalman_filter/motion_kalmanfilter.h"
#include "postprocess/kalman_filter/kalman_filter.h"
#include "postprocess/common/measurer.h"
#include "postprocess/type_fuser/sequence_typefuser.h"

// #define PREVIOUS_FLITER

class ObjectTrack {
 public:
  explicit ObjectTrack(TrackedObjectPtr obj);
  explicit ObjectTrack(TrackedObjectPtr obj,
                       PostProcessorSettings const& settings);
  ObjectTrack(ObjectTrack const&) = delete;
  ObjectTrack& operator=(ObjectTrack const&) = delete;
  ObjectTrack(ObjectTrack&&) = delete;
  ObjectTrack& operator=(ObjectTrack&&) = delete;
  ~ObjectTrack();

  /// @brief inline function : get kalman filter pointer
  /// @param void
  /// @return void
  // inline std::shared_ptr<KalmanFilter> Get_Kalman_Filter() { return filter_;
  // }

  /// @brief init id generator
  // static void InitIdGenerator() noexcept { id_generator_.Init(); };

  /// @brief predict the state of track
  /// @param[IN] time_diff: time interval for predicting
  /// @return predicted states of track
  Eigen::VectorXf Predict(double time_diff);

  /// @brief update track with object
  /// @param[IN] new_object: recent detected object for current updating
  /// @param[IN] time_diff: time interval from last updating
  /// @param[IN] is_utm_flag: flag of using utm or not
  /// @return nothing
  void UpdateWithObject(TrackedObjectPtr* new_object, double time_diff);

  /// @brief update track without object with given predicted state
  /// @param[IN] predict_state: given predicted state of track
  /// @param[IN] time_diff: time interval form last updating
  /// @return nothing
  void UpdateWithoutObject(Eigen::VectorXf const& predict_state,
                           double time_diff);

  /// @brief predict the state of track
  /// @param[IN] time_diff: time interval for predicting
  /// @return predicted states of track
  void SetID(uint16_t id) noexcept { idx_ = id; }

  /// @brief states
  // bool is_static_hypothesis_;
  Eigen::Vector3f belief_center_point_;
  Eigen::Vector3f belief_velocity_;
  Eigen::Matrix3f belief_velocity_uncertainty_;
  Eigen::Vector3f belief_acceleration_;
  Eigen::Matrix3f belief_acceleration_uncertainty_;
  Eigen::Vector4f belief_state_;
  Eigen::Matrix4f belief_state_covariance_;

  /// @brief basic info
  uint16_t idx_;
  std::int32_t age_;
  std::int32_t total_visible_count_;
  std::int32_t consecutive_invisible_count_;
  double period_;

  /// @brief current tracked object
  TrackedObjectPtr current_object_;

  /// @brief history tracked object
  std::deque<TrackedObjectPtr> history_objects_;

  /// @brief
  int fused_type_;

 private:
  /// @brief initialize tracker_object
  /// @param void
  /// @return void
  void init();

  /// @brief smooth velocity over track history
  /// @params[IN]: new_object: new detected object for updating
  /// @params[IN]: time_diff: time interval form last updating
  /// @return nothing
  // void smooth_track_velocity(TrackedObjectPtr const& new_object,
  //                            double time_diff);
  /// @brief check whether track is static or not
  /// @param[IN] new_object: new detected object just updated
  /// @param[IN] time_diff: time interval between last two updating
  /// @return true if track is static, otherwise return false
  // bool check_track_static_hypothesis(Lidar_Object const& new_object,
  //                                    double time_diff);

  /// @brief sub strategy of checking whether track is static or not
  /// @via considering the velocity angle change
  /// @param[IN] new_onject: new detected object just updated
  /// @param[IN] time_diff: time interval between last two updating
  /// @return true if track is static, otherwise retyurn false
  // bool check_track_static_hypothesis_by_velocity_angle_change(
  //     Lidar_Object const& new_object, double time_diff);

  // global parameters setup
  /// @brief Id Generator
  // static IdGenerator id_generator_;
  /// @brief track entire cached history count maximum
  static std::int32_t s_track_cached_history_size_maximum_;
  /// @brief speed noise maximum for smoothing velocity
  static float s_speed_noise_maximum_;
  /// @brief acceleration noise maximum for smoothing velocity
  static float s_acceleration_noise_maximum_;
  /// @brief kalman filter
#ifdef PREVIOUS_FLITER
  std::shared_ptr<KalmanFilter> filter_;
#else
  std::shared_ptr<MotionKalmanFilter> motion_filter_;
#endif
  /// @brief measurer
  Measurer measurer_;
  /// @brief type fuser
  std::shared_ptr<SequenceTypeFuser> type_fuser_;
};
using ObjectTrackPtr = ObjectTrack*;

class ObjectTrackSet {
 public:
  ObjectTrackSet();
  ~ObjectTrackSet();
  ObjectTrackSet(ObjectTrackSet const&) = delete;
  ObjectTrackSet& operator=(ObjectTrackSet const&) = delete;
  ObjectTrackSet(ObjectTrackSet&&) = delete;
  ObjectTrackSet& operator=(ObjectTrackSet&&) = delete;

  /// @brief set track consecutive invisible maximum
  /// @param[IN] track_consecutive_invisible_maximum: track consecutive
  /// invisible maximum
  /// @return true if set successfully, otherwise return false
  static bool SetTrackConsecutiveInvisibleMaximum(
      std::int32_t track_consecutive_invisible_maximum);

  /// @brief set track visible ratio minimum
  /// @param[IN] track_visible_ratio_minimum: track visible ratio minimum
  /// @return true if set successfully, otherwise return false
  static bool SetTrackVisibleRatioMinimum(float track_visible_ratio_minimum);

  /// @brief get maintained tracks
  /// @return maintained tracks
  inline std::vector<ObjectTrackPtr>& GetTracks() { return tracks_; }

  /// @brief get maintained tracks
  /// @return maintained tracks
  inline const std::vector<ObjectTrackPtr>& GetTracks() const {
    return tracks_;
  }

  /// @brief get size of maintained tracks
  /// @return size(number) of maintained tracks
  inline std::int32_t size() const { return tracks_.size(); }

  /// @brief add track to current set of maintained tracks
  /// @param[IN] track: adding track
  /// @return nothing
  void AddTrack(ObjectTrackPtr const& track) { tracks_.push_back(track); }

  /// @brief remove lost tracks
  /// @return number of removed tracks
  std::int32_t RemoveLostTracks() noexcept(false);

  /// @brief clear maintained tracks
  /// @return nothing
  void Clear();

  /// @brief
  static std::int32_t s_track_consecutive_invisible_maximum_;
  /// @brief
  static float s_track_visible_ratio_minimum_;

 private:
  /// @brief tracks
  std::vector<ObjectTrackPtr> tracks_;
};  // class Track_Object_Set

#endif
